06. A*: Shortest Path

A*: Shortest Path

You’ll now implement the A* algorithm and find the shortest path by modifying your previous code. As you know, A* is based on a heuristic function. Thus, we will implement a Manhattan-based heuristic vector and compute the Manhattan distance of each cell with respect to the goal position where:

x_{d} = x_{goal} - x_{cellposition}
y_{d} = y_{goal} - y_{cellposition}
Manhattan \ distance \ d = \left | x_{d} \right | +\left | y_{d} \right |

By computing the Manhattan distance of each cell, we'll obtain this heuristic vector

**Manhattan Heuristic Vector**
9 8 7 6 5 4
8 7 6 5 4 3
7 6 5 4 3 2
6 5 4 3 2 1
5 4 3 2 1 0

You can always experiment with other heuristic-based vectors such as the Euclidean distance or the Chebyshev distance where:

Euclidean \ distance \ d = \sqrt{ x_{d}^{2} + y_{d}^{2}}
**Euclidean Heuristic Vector**
6 5 5 4 4 4
5 5 4 3 3 3
5 4 3 2 2 2
5 4 3 2 1 1
5 4 3 2 1 0
Chebyshev \ distance \ d = max( \left | x_{d} \right |, \left | y_{d} \right |)
**Chebyshev Heuristic Vector**
5 4 4 4 4 4
5 4 3 3 3 3
5 4 3 2 2 2
5 4 3 2 1 1
5 4 3 2 1 0

Expansion

And now instead of expanding cells with lowest path cost g, you’ll expand cells with lowest f value which is the sum of the path cost g and the heuristic value h of that cell.

f = g + h

Each cell is now represented with a quadruplet [f,g,x,y] instead of a triplet [g,x,y].

A*: Shortest path

Task Description:

Follow these steps and make the necessary changes to find the shortest path using the A* algorithm:

Task List:

Task Feedback:

Great Job!

Hint

Here's how the cells are being expanded with the A* algorithm until the goal is reached:

Map 0 1 2 3 4 5
0 0 1 0 0 0 0
1 0 1 0 0 0 0
2 0 1 0 0 0 0
3 0 1 0 0 0 0
4 0 0 0 1 1 0

Expansion #: 0

Open List:
[9 0 0 0 ]


Cell Picked:
[9 0 0 0]


Expansion #: 1

Open List:
[9 1 1 0 ]


Cell Picked:
[9 1 1 0]


Expansion #: 2

Open List:
[9 2 2 0 ]


Cell Picked:
[9 2 2 0]


Expansion #: 3

Open List:
[9 3 3 0 ]


Cell Picked:
[9 3 3 0]


Expansion #: 4

Open List:
[9 4 4 0 ]


Cell Picked:
[9 4 4 0]


Expansion #: 5

Open List:
[9 5 4 1 ]


Cell Picked:
[9 5 4 1]


Expansion #: 6

Open List:
[9 6 4 2 ]


Cell Picked:
[9 6 4 2]


Expansion #: 7

Open List:
[11 7 3 2 ]


Cell Picked:
[11 7 3 2]


Expansion #: 8

Open List:
[13 8 2 2 ],
[11 8 3 3 ]


Cell Picked:
[11 8 3 3]


Expansion #: 9

Open List:
[13 9 2 3 ],
[13 8 2 2 ],
[11 9 3 4 ]


Cell Picked:
[11 9 3 4]


Expansion #: 10

Open List:
[13 10 2 4 ],
[13 9 2 3 ],
[13 8 2 2 ],
[11 10 3 5 ]


Cell Picked:
[11 10 3 5]


Expansion #: 11

Open List:
[13 11 2 5 ],
[13 10 2 4 ],
[13 9 2 3 ],
[13 8 2 2 ],
[11 11 4 5 ]


Cell Picked:
[11 11 4 5]


Start Quiz:

#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>

using namespace std;

// TODO: Add a Manhattan-based heuristic vector to the Map class
class Map {
public:
    const static int mapWidth = 6;
    const static int mapHeight = 5;
    vector<vector<int> > grid = {
        { 0, 1, 0, 0, 0, 0 },
        { 0, 1, 0, 0, 0, 0 },
        { 0, 1, 0, 0, 0, 0 },
        { 0, 1, 0, 0, 0, 0 },
        { 0, 0, 0, 1, 1, 0 }
    };
};

// Planner class
class Planner : Map {
public:
    int start[2] = { 0, 0 };
    int goal[2] = { mapHeight - 1, mapWidth - 1 };
    int cost = 1;

    string movements_arrows[4] = { "^", "<", "v", ">" };

    vector<vector<int> > movements{
        { -1, 0 },
        { 0, -1 },
        { 1, 0 },
        { 0, 1 }
    };
};

// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
    for (int i = 0; i < Vec.size(); ++i) {
        for (int j = 0; j < Vec[0].size(); ++j) {
            cout << Vec[i][j] << ' ';
        }
        cout << endl;
    }
}

/* #### TODO: Modify the search function and implement the A* algorithm #### */
void search(Map map, Planner planner)
{
    // Create a closed 2 array filled with 0s and first element 1
    vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
    closed[planner.start[0]][planner.start[1]] = 1;

    // Create expand array filled with -1
    vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));

    // Create action array filled with -1
    vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));

    // Defined the triplet values
    int x = planner.start[0];
    int y = planner.start[1];
    int g = 0;

    // Store the expansions
    vector<vector<int> > open;
    open.push_back({ g, x, y });

    // Flags and counters
    bool found = false;
    bool resign = false;
    int count = 0;

    int x2;
    int y2;

    // While I am still searching for the goal and the problem is solvable
    while (!found && !resign) {
        // Resign if no values in the open list and you can't expand anymore
        if (open.size() == 0) {
            resign = true;
            cout << "Failed to reach a goal" << endl;
        }
        // Keep expanding
        else {
            // Remove triplets from the open list
            sort(open.begin(), open.end());
            reverse(open.begin(), open.end());
            vector<int> next;
            // Stored the poped value into next
            next = open.back();
            open.pop_back();

            x = next[1];
            y = next[2];
            g = next[0];

            // Fill the expand vectors with count
            expand[x][y] = count;
            count += 1;

            // Check if we reached the goal:
            if (x == planner.goal[0] && y == planner.goal[1]) {
                found = true;
                //cout << "[" << g << ", " << x << ", " << y << "]" << endl;
            }

            //else expand new elements
            else {
                for (int i = 0; i < planner.movements.size(); i++) {
                    x2 = x + planner.movements[i][0];
                    y2 = y + planner.movements[i][1];
                    if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
                        if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
                            int g2 = g + planner.cost;
                            open.push_back({ g2, x2, y2 });
                            closed[x2][y2] = 1;
                            action[x2][y2] = i;
                        }
                    }
                }
            }
        }
    }
    // Print the expansion List
    //print2DVector(expand);

    // Find the path with robot orientation
    vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));

    // Going backward
    x = planner.goal[0];
    y = planner.goal[1];
    policy[x][y] = '*';

    while (x != planner.start[0] or y != planner.start[1]) {
        x2 = x - planner.movements[action[x][y]][0];
        y2 = y - planner.movements[action[x][y]][1];
        policy[x2][y2] = planner.movements_arrows[action[x][y]];
        x = x2;
        y = y2;
    }

    // Print the path with arrows
    print2DVector(policy);
}

int main()
{
    // Instantiate map and planner objects
    Map map;
    Planner planner;

    // Search for the expansions
    search(map, planner);

    return 0;
}
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>

using namespace std;

// Map class
class Map {
public:
    const static int mapWidth = 6;
    const static int mapHeight = 5;
    vector<vector<int> > grid = {
        { 0, 1, 0, 0, 0, 0 },
        { 0, 1, 0, 0, 0, 0 },
        { 0, 1, 0, 0, 0, 0 },
        { 0, 1, 0, 0, 0, 0 },
        { 0, 0, 0, 1, 1, 0 }
    };

    vector<vector<int> > heuristic = {
        { 9, 8, 7, 6, 5, 4 },
        { 8, 7, 6, 5, 4, 3 },
        { 7, 6, 5, 4, 3, 2 },
        { 6, 5, 4, 3, 2, 1 },
        { 5, 4, 3, 2, 1, 0 }
    };
};

// Planner class
class Planner : Map {
public:
    int start[2] = { 0, 0 };
    int goal[2] = { mapHeight - 1, mapWidth - 1 };
    int cost = 1;

    string movements_arrows[4] = { "^", "<", "v", ">" };

    vector<vector<int> > movements{
        { -1, 0 },
        { 0, -1 },
        { 1, 0 },
        { 0, 1 }
    };
};

// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
    for (int i = 0; i < Vec.size(); ++i) {
        for (int j = 0; j < Vec[0].size(); ++j) {
            cout << Vec[i][j] << ' ';
        }
        cout << endl;
    }
}

// Search function will generate the expansions
void search(Map map, Planner planner)
{
    // Create a closed 2 array filled with 0s and first element 1
    vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
    closed[planner.start[0]][planner.start[1]] = 1;

    // Create expand array filled with -1
    vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));

    // Create action array filled with -1
    vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));

    // Defined the quadruplet values
    int x = planner.start[0];
    int y = planner.start[1];
    int g = 0;
    int f = g + map.heuristic[x][y];

    // Store the expansions
    vector<vector<int> > open;
    open.push_back({ f, g, x, y });

    // Flags and Counts
    bool found = false;
    bool resign = false;
    int count = 0;

    int x2;
    int y2;

    // While I am still searching for the goal and the problem is solvable
    while (!found && !resign) {
        // Resign if no values in the open list and you can't expand anymore
        if (open.size() == 0) {
            resign = true;
            cout << "Failed to reach a goal" << endl;
        }
        // Keep expanding
        else {
            // Remove quadruplets from the open list
            sort(open.begin(), open.end());
            reverse(open.begin(), open.end());
            vector<int> next;
            // Stored the poped value into next
            next = open.back();
            open.pop_back();

            x = next[2];
            y = next[3];
            g = next[1];

            // Fill the expand vectors with count
            expand[x][y] = count;
            count += 1;
            
            // Check if we reached the goal:
            if (x == planner.goal[0] && y == planner.goal[1]) {
                found = true;
                //cout << "[" << g << ", " << x << ", " << y << "]" << endl;
            }

            //else expand new elements
            else {
                for (int i = 0; i < planner.movements.size(); i++) {
                    x2 = x + planner.movements[i][0];
                    y2 = y + planner.movements[i][1];
                    if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
                        if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
                            int g2 = g + planner.cost;
                            f = g2 + map.heuristic[x2][y2];
                            open.push_back({ f, g2, x2, y2 });
                            closed[x2][y2] = 1;
                            action[x2][y2] = i;
                        }
                    }
                }
            }
        }
    }

    // Print the expansion List
    print2DVector(expand);

    // Find the path with robot orientation
    vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));

    // Going backward
    x = planner.goal[0];
    y = planner.goal[1];
    policy[x][y] = '*';

    while (x != planner.start[0] or y != planner.start[1]) {
        x2 = x - planner.movements[action[x][y]][0];
        y2 = y - planner.movements[action[x][y]][1];
        policy[x2][y2] = planner.movements_arrows[action[x][y]];
        x = x2;
        y = y2;
    }

    // Print the robot path
    cout << endl; 
    print2DVector(policy);
}

int main()
{
    // Instantiate a planner and map objects
    Map map;
    Planner planner;

    search(map, planner);

    return 0;
}